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A catalog of inverse-kinematics planners 
for underactuated systems on matrix groups 
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Abstract — This paper presents motion planning algorithms for un- 
deractuated systems evolving on rigid rotation and displacement 
groups. Motion planning is transcribed into (low-dimensional) com- 
binatorial selection and inverse-kinematics problems. We present a 
catalog of solutions for all underactuated systems on SE(2), SO(3) and 
SE(2) x K classified according to their controllability properties. 
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I. Introduction 

This paper presents motion planning algorithms for 
kinematic models of underactuated mechanical systems; 
we consider kinematic (i.e., driftless) models that are 
switched control systems, that is, dynamical systems de- 
scribed by a family of admissible vector fields and a con- 
trol strategy that governs the switching between them. 
In particular, we focus on families of left-invariant vector 
fields defined on rigid displacements subgroups. 

This class of systems arises in the context of kinematic 
modeling and kinematic reductions for mechanical con- 
trol systems; see the recent works Q, Q, 0, |H, 151 . 
Clearly, the transcription into kinematic models simplifies 
the motion planning problem; e.g., |4J discusses 3R planar 
manipulators and 1 6 1, 1 7 1 discuss the snakeboard system. 

Literature review 

Motion planning for kinematic models, sometimes re- 
ferred to as driftless or nonholonomic models, is a classic 
problem in robotics; see 1 8 1 and also the references therein. 
In particular, the algorithms in |9|, |10|, |11J focus on dy- 
namical aspects and exploit controllability properties. 

For the class of systems of interest in this paper, the 
search for a motion planning algorithm is closely re- 
lated to the inverse-kinematics problem. Example inverse- 
kinematics methods include (i) iterative numerical meth- 
ods for nonlinear optimization, see |12|, (ii) geomet- 
ric and decoupling methods for classes of manipulators, 
see 1131 . 1141 , (iii) the Paden-Kahan subproblems ap- 
proach, see 1151 . 1111 . and (iv) the general polynomial pro- 
gramming approach, see 1161 . The latter and more gen- 
eral method is based on tools from algebraic geometry 
and relies on simultaneously solving systems of algebraic 
equations. Despite these efforts, no general methodology 
is currently available to solve these problems in closed- 
form. Accordingly, it is common to provide and catalog 

Departamento de Matematica Aplicada IV, Universidad Politecnica 
de Cataluna, Av. V. Balaguer, s/n, Vilanova i la Geltru, 08800, Spain, 
email: soniam@mat . upc . es 

Coordinated Science Laboratory, University of Illinois 
at Urbana-Champaign, Urbana, IL 61801, United States, 
email: { jcortes, bullo}@uiuc . edu 



closed-form solutions for classes of relevant example sys- 
tems; see El, El, EH- 

Problem statement 

We consider left-invariant control systems evolving on 
a matrix Lie subgroup G c SE(3). Examples include sys- 
tems on SE(2), SO (3) and SE(2) x R. As usual in Lie group 
theory, we identify left-invariant vector fields with their 
value at the identity. Given a family of left-invariant vec- 
tor fields {Vi, . . . , V m } on G, consider the associated drift- 
less control system 



9{t)=J2v i (g(t))u i [t). 



(1) 



where g : R — > G and where the controls (iti, . . . , u m ) take 
value in {(±1, 0, . . . , 0), (0, ±1, 0, . . . , 0), . . . , (0, . . . , 0, ±1)}. 
For these systems, controllability can be assessed by alge- 
braic means: it suffices to check the lack of involutivity of 
span{Vi, . . . , V m }. Recall that for matrix Lie algebras, Lie 
brackets are matrix commutators [A, B] = AB — BA. 

This paper addresses the problem of how to compute 
feasible motion plans for the control system Q by con- 
catenating a finite number of flows along the input vec- 
tor fields. We call a flow along any input vector field a 
motion primitive and its duration a coasting time. There- 
fore, motion planning is reduced to the problem of se- 
lecting a finite-length combination of k motion primi- 
tives . . . , ik) £ {1, ■ • ■ , m} k and computing appropri- 
ate coasting times (£i, . . . , tk) £ K fc that steer the system 
from the identity in the group to any target configuration 
<7f G G. In mathematical terms, we need to solve 

gt = exp^iVi! ) • ■ • exp(t k V ik ). 

Hence, motion planning is transcribed into low-dimensional 
combinatorial selection and inverse-kinematics problems. 

Contribution 

The contribution of this paper is a catalog of solutions 
for underactuated example systems defined on SE(2), 
SO (3), or SE(2) x K. Based on a controllability analysis, we 
classify families of underactuated systems that pose qual- 
itatively different planning problems. For each family, we 
solve the planning problem by providing a combination 
of k motion primitives and corresponding closed-form ex- 
pressions for the coasting times. In each case, we attempt 
to select k = dim(G): generically, this is the minimum nec- 
essary (but sometimes not sufficient) number of motion 
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primitives needed. If the motion planning algorithm en- 
tails exactly dim(G) motion primitives, i.e., minimizes the 
number of switches, we will refer to it as a switch-optimal 
algorithm. Sections HD|^ and |IV] present switch-optimal 
planners for SE(2), SO(3), and SE(2) x R, respectively. 

Notation 

Here we briefly collect the notation used throughout the 
paper. Let S be a set, ids ■ S —> S denote the identity map 
on S and let inds : R — > M denote the characteristic func- 
tion of S, i.e., inds(x) = 1 if x £ S and inds(x) = if 
x £ S. Let arctan2 (x, y) denote the arctangent of y/x tak- 
ing into account which quadrant the point (x, y) is in. We 
make the convention arctan2 (0, 0) = 0. Let sign : R — > R 
be the sign function, i.e., sign(a;) = 1 if x > 0, sign(aj) = — 1 
if x < and sign(0) = 0. Let A t j be the (i, j) element of the 
matrix A. Given v, w G R", let arg(v, if) £ [0, it] denote the 
angle between them. Let || • || denote the Euclidean norm. 

Given a family of left-invariant vector fields {Vi, . . . , V m } 
on G, we associate to each multiindex (ix,...,ik) £ 
{l,...,m} k the forward-kinematics map J r JG- tl ''"' ik ^ : 
R fe -> G given by {t x , ...,t k )i-> exp^iVL)- ■■exp(t k V ih ). 

II. Catalog for SE(2) 
Let {eg, e Xl e y } be the basis of se(2): 
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, [e y , eg] 




e x and 




= 0. 


For ease 



of presentation, we write V £ se(2) as V 
ce y = (a, b, c), and g € SE(2) as 



aeg 



9 



cos( 
sinf 




— sin 9 x 
cos 9 y 
1 



(6,x,y) . 

With this notation, cxp : se(2) -> SE(2) is 



exp(a, 6, c) 



sin a 
a , o 



1 



cos a 1 
c, — 



cos a. 



sm a 



for a ^ 0, and exp(0, 6, c) = (0, b, c). 

Lemma II.l: (Controllability conditions). Consider two 
left-invariant vector fields V\ = (ax,bx,cx) and V 2 = 
(02,62,02) in se(2). Their Lie closure is full rank if and 
only if 0162 — b\a 2 7^ or c\a,2 — a\c 2 7^ 0. 

Proof: Given [Vi, V 2 ] = (0, cia 2 - aic 2 , aib 2 - ha 2 ), 
one can see that span { Vi , V 2 , [Vi , V 2 ] } = se(2) if and only if 



det 



ax bi ci 

a 2 b 2 c 2 

c\a 2 - c 2 a\ b 2 ai - b\a 2 



(a\b 2 - bia 2 ) 2 + {c\a 2 - aic 2 ) 2 7^ . 



Let Vx = (ax,bx,cx) and V 2 = (a 2 ,b 2 ,c 2 ) satisfy the con- 
trollability condition in Lemma 111.11 Accordingly, either 
ai or a 2 is different from zero. Without loss of generality, 
we will assume that a x 7^ 0, and take ax = 1. As a conse- 
quence of Lemma flLll there are two qualitatively different 
cases to be considered: 



= 1} 



51 = {(Vx,V 2 ) £ «(2) x «(2) | Vi = (1, bx,cx), 

V 2 = (0,6 2 ,c 2 )and62 

5 2 = {(Vx,V 2 ) G se(2) x ae(2) | Vi = (1, 6 1; Cl ), 

V2 = (1, b 2l c 2 ) and either 61 7^ 6 2 orci 7^ c 2 } . 



Since dim(se(2)) = 3, we need at least three motion prim- 
itives along the flows of {Vx, V 2 } to plan any motion be- 
tween two desired configurations. Consider the map 
J7C (1 ' 2,1) : R 3 -> SE(2). In the following propositions, we 
compute solutions for Si and e> 2 -systems. 

Proposition II. 2: (Inversion for Si -systems on SE(2)). 
Let (Vx,V 2 ) G Si. Consider the map T/C[5i] : SE(2) -> M 3 , 

2"/C[<Si](0, x, y) = (arctan2 (a, (3) , p,9 — arctan2 (a, (3)), 



where p = \J a 2 + f3 2 and 








b 2 c 2 
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-ci bi 




1 — cos 9 
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-c 2 b 2 _ 




y_ 




bi ci 




sin# 





Then, X/C[iSi] is a global right inverse of TT0 1,2,1 \ that is, 
it satisfies TK, {1 ^ 1] o!K\S x ] = id SE(2) : SE(2) -> SE(2). 
Note that the algorithm provided in the proposition is not 
only switch-optimal, but also works globally. 

Proof: The proof follows from the expression of the 
map Let rKS 1 ' 2 '^ {t x , t 2 , t 3 ) = (9,x,y), 

6 = h+t 3 , 



X 




-ci bi 




1 — cos 9 
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~b 2 


—c 2 




costi 


y_ 




bi ci_ 




sin 9 




p 2 


b 2 




sinii 



The equation in [x,y] T can be rewritten as [a,/3] T = 
[costx,s'mti] T t 2 . The selection t\ = arctan2 (a, /3), t 2 = p 
solves this equation. ■ 
Proposition II. 3: (Inversion for <S2-systems on SE(2)). 
Let (Vi,V 2 ) G 6>2. Define the neighborhood of the iden- 
tity in SE(2) 

U = {(9, x, y) G SE(2) | ||(ci - c 2 , h - 6 2 )|| 2 > 

max{||( 2 ;,y)|| 2 ,2(l-cos0)||(6i, C i)|| 2 }. 

Consider the map 1K[S 2 ] : U c SE(2) -> R 3 whose com- 
ponents are 

XK.[S 2 ]i(6, x, y) = arctan2 (p, \J 4 — p 2 ^j + arctan2 (a, (5) , 

IIC[S 2 } 2 (9, x, y) = arctan2 (2 - p 2 , p^^-p 2 
UC[S 2 ] 3 (9, x,y)=9- lK,[S 2 ]i{6, x, y) - 1IC[S 2 ] 2 (9, x, y) , 
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and p = \f c 



j3 2 and 
1 



\\{c x -c 2 M-hW 



c\ - c 2 b 2 
h - b 2 ci 



bi 
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1 — cos 9 


) 




y. 




h ci 




sin 9 





Then, T/C[<S 2 ] is a local right inverse of TKS 1 ' 2 ' 1 ', that is, it 
satisfies ^/C (1 ' 24) oI/C[5 2 ] = idc/ : E7 -»• £/. 
Proq/: If (0,x,y) £ C/, then 



P= 11(^/3)11 < 



Cl - C2, &1 - 

Cl &1 



l(^y)ll + 



&i ci 



1 — cos ( 
sin0 



< 2. 



and hence 1JC[S 2 ] is well-defined on U. Let2JC[S 2 ](9,x,y) 
{ti,t 2 , t 3 ). The components of FfC^ 1 ' 2 ' 1 '^, t 2 , t^) are 

^ W) (tl J ta,<3)=*l+*2+*3, 

TK, 2 1,2 ' 1 \t 1 , t 2 , t 3 ) 



TIC 



(1,2,1) 



(£1,^2,^3) 





-Cl 






"1 


— cos 9 








. fo l 


Cl_ 






sin0 
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cosii 


— cos(i 


1 - 


\-t 2 )~ 


Cl 


- c 2 


sinti 


— sin(ti - 


-t 2 ) 



Cl 

b 2 



In an analogous way to the previous proof, one verifies 
T^ 1 ' 2 ' 1 \t 1 ,t 2 ,t 3 ) = (e,x,y). ■ 
Remark II. 4: The map UC[S 2 ] in Proposition 111.31 is a lo- 
cal right inverse to J7C' 1 ' 2 ' 1 ' on a domain that strictly con- 
tains U. In other words, our estimate of the domain of 
IK,\S 2 ] is conservative. For instance, for points of the form 
(0, x, y) e SE(2), it suffices to ask for 

|| (x, y) || < 2jj( Cl -C2, 6i-6 2 )||. 

For a point (0, 0, 0) e SE(2), it suffices to ask for 

(1 - cos0)||(6i,ci)|| 2 < 2||(ci - 02M - b 2 )f . 

Additionally, without loss of generality, it is convenient to 
assume that the vector fields V\ , V 2 satisfy b\ + c| < 6 2 + c 2 ,, 
so as to maximize the domain U . 

We illustrate the performance of the algorithms in Fig.Q] 

III. Catalog for SO(3) 
Let {e x ,e y ,e z } be the basis of so(3): 
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Here we make use of the notation V 



be,, 



ce z = (a, b, c) based on the Lie algebra isomorphism ^ : 
(R 3 , x) — > (bo(3), [•,•]). Rodrigues formula El for the ex- 
ponential cxp : so (3) — > SO (3) is 



cxp(^) 



13 H 11 11 V ' 

\\v\\ 



1-cos||t?|U 2 
hll 2 V 



S 1* 



Hg.l 

WE ILLUSTRATE THE INVERSE-KINEMATICS PLANNERS FOR S\ AND 
52-SYSTEMS. THE PARAMETERS OF BOTH SYSTEMS ARE 
(61, Cl) = (0, .5), (b 2 , C 2 ) = (1,0). THE TARGET LOCATION IS 

(71-/6, 1, 1). Initial and target location are depicted in dark 

GRAY. 



The commutator relations are [e^,e z ] = — e y , [e y ,e z ] = e x 
and [e x ,e y ] = e z . 

Lemma III.l: (Controllability conditions). Consider two 
left-invariant vector fields V\ = (ai,6i,ci) and V 2 = 
(a 2 , 62 , C2 ) in so (3) . Their Lie closure is full rank if and only 
if ci<22 — aic 2 ^ or &ic 2 — ci6 2 ^ or 6ia 2 — 0162 ^ 0. 

Proof: Given the equality [Vi,!^] = V\ x V 2 , with 
V\ x V 2 = (61 C2 - b 2 ci,aa 2 - c 2 ai,a\b 2 - 0261), one can 
see that span {Vi, V 2 , [Vi, V 2 ]} = so(3) if and only if 



dct 



n c 2 



ai 61 ci 

a 2 b 2 c 2 

6ic 2 - 6 2 ci cia 2 - c 2 a! ai6 2 - a 2 6i 

62C1) 2 + (cia 2 - c 2 ai) 2 + (ai6 2 



a 2 6i) 2 ^0. 



Let Vi, V 2 satisfy the controllability condition in 
Lemma llll.ll Without loss of generality, we can assume 
Vi = e z (otherwise we perform a suitable change of co- 
ordinates), and ||V 2 || = 1. In what follows, we let V 2 = 
(a, 6, c). Since e z and V 2 are linearly independent, neces- 
sarily a 2 + 6 2 / and c =/= ±1. Since dim(so(3)) = 3, we 
need at least three motion primitives to plan any motion 
between two desired configurations. Consider the map 
^•jC (1,2 ' 1) : M 3 -> SO(3),thatis 

^■/C (1,2 ' 1) (ii ) t2,t 3 ) = exp(tie z )exp(t 2 V 2 )exp{t 3 e z ). (2) 

Observe that equation 10 is similar to the formula for cer- 
tain sets of Euler angles; see 1111 . 

Proposition III. 2: (Inversion for systems on SO (3)). Let 
Vi = (0, 0, 1) and V 2 = (a, 6, c), with a 2 + 6 2 ^ and 
c ^ ±1. Define the neighborhood of the identity in SO (3) 

{R e SO(3) I i? 33 G [2c 2 



U 

Consider the map 2JC: U C SO(3) 
nents are 



-1,1]}. 
I 3 whose compo- 



IK-i(R) = arctan2 (wii?i3 + w 2 R 23 , —w 2 Ri 3 

n-v 1 m ( - c 2 

2K, 2 (R) = arccos 



wii? 23 ) , 



1 -c 2 

X/C3 (-R) = arctan2 (V1R31 ■ 



v 2 R 32 ,v 2 R 31 - viR 32 ) 
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where, for z = (1 - cos(Z/C 2 (i?)), sm(I/C 2 (i?))) T , 







ac b 




Vl 




ac —b 


W 2 




cb —a 






cb a 



Then, XK, is a local right inverse of TKS 1 ' 2 ' 1 
satisfies .F/C (1 ' 24) o TK = id v : U -> U. 

Proof: Let R e U. Then, R ^_~f I 
hence XIC(R) is well-defined. Denote £j 
and let us show i? = TIC ih2 ' 1) (t lt t 2 , t 3 ). 
the rows (resp. the columns) of a rotation matrix con- 
sist of orthonormal vectors in R 3 . Therefore, the ma- 
trix TlC {1 ' 2 - 1) (t 1 ,t 2 ,t 3 ) e SO(3) is determined by its 
third column FfC^ 1 ' 2 ' 1 ' (t±, t 2 , t 3 )e z and its third row 
e^/C (1,2,1) (ti,t 2 , fe). The factors in admit the follow- 
ing closed-form expressions. For c t = cos t and s f = sin t, 



Two sufficient conditions are also meaningful. In terms of 
the rotation angle, if \6\ < arccos(2c 2 — 1) then 1 — cos0 < 
2(1 — c 2 ), and in turn equation j3J is satisfied. In terms of 
the axis of rotation, a sufficient condition for equation 
is sin 2 (arg(e z ,w)) < sin 2 (arg(e z , V 2 )) = 1 -c 2 . 
, that is, it We in us t ra te the performance of the algorithm in Fig.0 

< 1, and 

= HQ(R) 
Recall that 



exp(te z ) 



<■■ -s t 
s t c t 
1 



and exp(iV^) equals 

a 2 + (1 — a?)c t ba(l — c t ) - est ca(l - ct) + bs t 



o6(l - c t ) + cs t 6 2 + (1 - b 2 )c t cb(l - c t ) 
ac(l — ct) — 6s 



bc(l - c t ) + as t c 2 + (1 - c 2 )c t 

Now, using the fact that exp(ie z )e z = e z , we get 

J"/C (1 ' 2 - 1) (*i, i 2 , ^)e z = cxp(tie z ) cxp(t 2 V 2 ) cxp(i 3 e z )e z 
= exp(*ie*) exp(t 2 V r 2 )e z = exp(^e z ) 



W2 
R33 



Re z 



A similar computation shows that eJJ 7 K.^ 1 ' 2,1 '(ti, t 2l t 3 ) = 
e T i?, which concludes the proof. ■ 
Remark III. 3: If e z and V 2 are perpendicular, then U = 
SO (3) and the map 2X is a global right inverse of 
■pfci 1,2,1) _ Otherwise, let us provide an equivalent formu- 
lation of the constraint i?33 e [2c 2 — 1,1] in terms of the 
axis/ angle representation of the rotation matrix R. Recall 
that there always exist a, possibly non-unique, rotation an- 
gle 9 e [0, 7r] and an unit-length axis of rotation ui £ E> 2 
such that R = cxp(Q9). Because lq 2 = lo t ui — I 3 , an equiv- 
alent statement of Rodrigues formula is 

R = I 3 +ujsin9 + (1 - cos9)(lu t lu - 7 3 ). 

From ef,uj = cos(arg(e z , u)), we compute 

e T z Re z = e T z e z + (1 — cos 9)((e^Lo) 2 — eje z ) 
= l + (l-cos6»)((efw) 2 -l) 
= 1 — sin 2 (arg(e z , cj))(1 — cos#) . (3) 

Therefore, R 33 e [2c 2 — 1, 1] if and only if 

1 - sin 2 (arg(e z ,w))(l - cos6>) > 2c 2 - 1 

sin 2 (arg(e z ,w))(l -cos(9) < 2(1 - c 2 ) . 




Fig. 2 

We illustrate the inverse-kinematics planner on SO (3). The 

SYSTEM PARAMETERS ARE (a,b, c) = (0, 1/V2, l/Vfy, THE TARGET 
FINAL ROTATION IS exp(7r/3, 7r/3, 0). TO RENDER THE SEQUENCE OF 
THREE ROTATIONS VISIBLE, THE BODY IS TRANSLATED ALONG THE 
INERTIAL X-AXIS. 



IV. Catalog for SE(2) x R 

Let {(e e ,0),(e x ,0),(e,,,0), (0,0,0, 1)} be a basis of 
se(2) x R, where {eg, e x , e y , } stands for the basis of se(2) 
introduced in Section |IJ With a slight abuse of notation, 
we let eg denote (eg, 0), and we similarly redefine e x and 
e y . We also let e z = (0, 0, 0, 1). The only non-vanishing Lie 
algebra commutators are [eg, e x ] = e y and [eg, e y ] = — e x . 

A left-invariant vector field in se(2) x Ris written as 
V = aeg + be x + ce y + de z = (a, b, c, d), and g 6 SE(2) xR as 
g = (9, x, y, z). The exponential map, exp : St(2) x R — ► 
SE(2) x R, is given component-wise by the exponential on 
se(2) and R, respectively. That is, exp(V) is equal to 



sin a 1 — cos a 1 
a, o c, — 



cos a , 



sin a 



c , d 



if a =/= 0, and cxp(V) = (0, b, c, d) if a = 0. 

Lemma IV. 1: (Controllability conditions for systems in 
SE(2) x R with 2 inputs). Consider two left-invariant vec- 
tor fields Vi = (ai, bi, ci, d\) and Vi — (a 2 , b 2 , c 2 , d 2 ) in 
se(2) x R. Their Lie closure is full rank if and only if a 2 d\ — 
d 2 a\ ^ 0, and either cia 2 — aic 2 ^ or a\b 2 — b\a 2 ^ 0. 

Proof: Since [Vi,V^] = (0,cia 2 — a\c 2 ,a\b 2 — 
b\a 2 ,0) 7^ 0, we deduce that either cia 2 — aic 2 ^ or 
a\b 2 — b\a 2 7^ 0. In particular, this implies that necessarily 
ai or a 2 ^ 0. Assume ai 7^ 0. Now, 

[Vi, [Vi, V 2 }] = (0,ai(-6 2 ai + bia 2 ), ai(cia 2 - c 2 ai),0) , 

and note that [V 2 ,[V U V 2 ]} = (a 2 /ai)[Vi, [Vi, V 2 }]. Finally, 
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Lie({Vi, V 2 }) = se(2) x R if and only if 



A. Two-dimensional input distribution 
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b 2 ai - bxa-2 
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+ ha 2 ) 


ai{c\a 2 — c 2 a\) 






det 



ai(a 2 di - d 2 ai) [{c\a 2 - c 2 ai) 2 + {-b 2 ai + bia 2 ) 2 ] ^ 0. 

Since [Vi, V 2 ] ^ 0, this reduces to a 2 di — d 2 ai ^ 0. ■ 

Let V\, V 2 satisfy the controllability condition in 
Lemma llV.ll Without loss of generality we can assume 
ai = 1. As in the case of SE(2), there are two qualitatively 
different situations to be considered: 

T 1 = {(V 1 ,V 2 ) G (se(2) x R) 2 | Vi = (1, b u Cl , d{), 

V 2 = (0,6 2 ,c 2 ,l)and6 2 +c 2 ^0}, 
T 2 = {(V U V 2 ) G (se(2) x R) 2 | Vi = (l.fc.d.di), 

V 2 = (1, b 2 ,c 2 ,d 2 ), d\ ^ d 2 and either bi ^ b 2 or ci ^ c 2 } . 

Lemma IV.2: (Controllability conditions for SE(2) x R 
systems with 3 inputs). Consider three left-invariant vec- 
tor fields Vi = (a,i,bi,Ci,di), i = 1,2,3 in se(2) x R. As- 
sume UedVi^V^}) C se(2) x R, for ij G {1,2,3} and 
Lie({^i,V2,y 3 }) = se(2) x R. Then, possibly after a re- 
ordering of the vector fields, they must fall in one of the 
following cases: 



T 3 = {(V ll V 2 ,V 3 ) & (sc(2) 



Vx = (l,6i,d,di),y a = 



(0, 6 2 ,c 2 ,0), V 3 = (l,b 1 ,c 1 ,d 3 ),d 1 ^ d 3 andb 



T 4 = {(Vi, V 2 , V 3 ) G (se(2) x R) 3 | Vi - (1, b u c u d x ), V 2 



(0,b 2 ,c 2 ,0),V 3 = {0,0,0,d 3 ),0^d 3 ^d 1 andb 



1^0}, 



{(Vi, V 2 , V 3 ) G (se(2) x R) 3 | Vi = (1, b u c u d x ), 

V 2 = (1, b 2 , c 2 ,dx),V 3 = (0, 0, 0, d 3 ), d 3 ^ 

and either b 2 ^ b\ or c.\ ^ c 2 }. 
Proof: Without loss of generality, we can assume 
that [Vi,V 2 ] + and ai = 1. Since Lie ({Vi, V 2 }) ^ 
se(2) x R, then a 2 d\ = d 2 . Given that the Lie closure of 
{Vi, V 2 , V 3 } is full-rank, and dim(span {V U V 2 ,[V U V 2 }}) = 
3, we have that d 3 ^ a 3 d\. This latter fact, together with 
Lie({Vi,V 3 }) C se(2) x R, implies that [Vi,Vs] = 0, and 
therefore b 3 = a 3 b\, c\a 3 = c 3 . We now distinguish two 
situations depending on [V 2 , V 3 ] being zero or not. 

(a) [V"2,V3] ^ 0. Necessarily, a 3 ^ 0. Therefore, we can 
assume 03 = 1. Since Lic({V 2 , V 3 }) is not full-rank, then 
a 2 = 0. We then have a 73-system. 

(b) [V 2 , V3] = 0. Necessarily, b 3 a 2 = b 2 a 3 and c 2 a 3 = c 3 a 2 . 
Depending on the values of a 2 and 03, we consider: 

(i) If a 2 = a 3 = 0, then d 2 = 0, d 3 ^ 0, 63 = c 3 = 0. Then, 
this is a 7i -system. 

(ii) If a 2 = 0, and 03 = 1, then b 2 = b 3 a 2 = 0, c 2 = C3a 2 = 
and also d 2 = d\a 2 = 0. This is not possible as it would 
make V 2 = 0. 

(iii) If 02 = 1 and 03 = 0, then 63 = c 3 = 0, and d 2 = d\ . 
Therefore, this is a 75-system. 

(iv) Finally, if a 2 = 1 and a 3 = 1, then bi = b 2 , c\ = c 2 , 
and di = d 2 , which makes Vi and V2 linearly dependent. 



X 






+ 


ci bi 




cos 9 


_y_ 








-b x ci_ 




sin 9 



'b 2 


h + 


~b 2 


-c 2 




cost 2 


c 2 _ 


c 2 


b 2 




sint 2 



Let Vi, V 2 satisfy the controllability condition in 
Lemma HV.1I Since dim(se(2) x R) = 4, we need at 
least four motion primitives to plan any motion be- 
tween two desired configurations. Consider the map 
TIC {2 ' 1 - 2 ' 1) : R 4 -> SE(2) x R. 

Proposition TV.3: (Lack of switch-optimal inversion for 
Ti-systems on SE(2) x R). Let (Vi,V 2 ) G Then, the 
map JF/C' 2 ' 1,2 ' 1 -' in not invertible at any neighborhood of 
the origin. 

Proof: Let .f/d 2 - 1 ' 2 ' 1 ^!,^, £3,^4) = {0,x,y,z). Then, 
9 = t 2 + t4 , 

z = h + t 3 +dt(t 2 +U) = h+t 3 + dx9, 



Consider a configuration with 9 = z — 0. Then, the equa- 
tion in (x,y) is invertible if and only if the map / : R 2 — > 
R 2 defined by 



is invertible. But / can not be inverted in (0, /?), j3 ^ 0. ■ 

Remark TV A: An identical negative result holds if we 
start taking motion primitives along the flow of Vi in- 
stead of V 2l i.e., if we consider the map TK^ 1 ' 2,1 ' 2 ^ : R 4 — > 
SE(2) x R. 

Consider the map Jc/C^' 2 ' 1 ' 2 ' 1 ' : R 5 SE(2) x R. 

Proposition IV.5: (Inversion for 7i -systems on SE(2) x 
R). Let (Vi,y 2 ) G %,. Consider the map X/C[Ti]: SE(2) x 
R —> R 5 whose components are 

IIC[T 1 } 1 (9,x,y, z) = 7rind]_ OO)0 [(7 - p) + arctan2 (a,/3) 

+ arctan2((p + 7 )/2,0), 
lK[T 1 } 2 (6,x,y,z) = (j-p)/2, 
X/C[ri] 3 (0,x, 2/ , z) = arctan2 ((p 2 - 7 2 )/4,0) 

+ 7T (ind ] _ OOi0[ (7 + p) - ind ] _ OO:0[ (7 - p)) , 
TK\T^(Q,x,y,z) = (l + p)/2, 

llC[T 1 } 5 (9,x,y, z) =9-JJC[T 1 ] 1 (9,x,y,z)-I!C[T 1 ) 3 (9,x,y,z), 



't 2 




cost 2 — 1 


*3. 




sin ^2 



where p = \/ a 2 + (3 2 and 

7 = z — c?i 6* , 





1 


b 2 C2 




X 




-ci bi 




'l - cos 0" 




A 


~ b\ + c\ 


-c 2 b 2 _ 


( 


y. 




bi ci 




sin 9 


) 



Then, 27C[7I] is a global right inverse of TlC il ' 2A ' 2 ' 1) , that 
is, it satisfies ^/C^' 2 ' 1 ' 2 ' 1 ' o I/C[7i] = id SE(2)xK : SE(2) x 
R -> SE(2) x R. 
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Proof: The proof follows from the expression of 

^£(1,2,1.2,1). Let ^/c (1 ' 2 ' 1 ^ 1) (ii,i 2 ,i 3 ,i4,i5) = {0,x,y,z), 

= ti + t a + h , 
z = t 2 + t 4 + d 1 9, 



h ci 

b 2 -c 2 
c 2 6 2 



1 — cos ( 
sin 8 

costi 
sin ti 



t 2 + 



cos(ti + £ 3 ) 
sin(ti + i 3 ) 



The equation in [x, y] T can be rewritten as 



Proof: If (6,x,y,z) G £/, then p < 4 and |-y | < 
2 arccos (— 1 + p/2). This in turn implies that 

c = cos(l)>-l + P ->-l + ^ 

over p < 4. The second inequality guarantees that 
I is well-defined. The first one implies I G [p — 
2,2], which makes 1JC[T 2 ] well-defined on U. Let 

XK.[T 2 ](6,x,y,z) = (£ l7 £ 2 , £ 3 , £ 4 , £ 5 ). The components of 
^(1,2,1,2,1) ( tut2i ^ t4j ts ) are t h e following 

9 = h + to + 1 3 + ti + u , 







cos£i 




COs(£i + £3) 




z = diO + 


(da 


-di)(*2 


f *4), 


O 




t 2 + 


ti , 














p. 




sinfi 


sin(ti + £ 3 )_ 


a; 




-ci 


+ 


ci 6i 




cos 


+ 












_y_ 




& 1 




-6i ci 




sin# 





which is solved by the selection of coasting times given by 
the components of the map 2X [71] . ■ 
Proposition IV.6: (Inversion for 7^-systems on SE(2) x 
R). Let (Vi, V 2 ) G 7^. Define the neighborhood of the iden- 
tity in SE(2) x K 



U 



{(8, x, y, z) G SE(2) x R | 4 ||(ci - c 2 , 6i - b 2 ) 



> 



maxi (x, 



, 2(1 - 008^)11(6!, Cl )|| 2 } 



\z — d\6\ < 2\d 2 — di \ arccos — 1 



1 



(ci - c 2l bi - ba)!! 
■(||(x,y)|| + ||(& 1 , Cl )||V2(l-cos0)))}. 

Consider the map 2X[T 2 ] : SE(2) xl^l 5 whose compo- 
nents are 

27C[72]i(0,x,y, z) = arctan2 f/, ^4 - l 2 ^j + arctan2 (a, 0) , 



X/C[T 2 ] 2 (6»,x, 2 /,z) =2arctan2fv / 4-Z 2 ,Zj , 

2X[T 2 ] 3 (<9, x, y, z) = - arctan2 (p - I, ^4 - (p - I) 2 

- IK[T 2 ]i{6, x, y, z) - HC[T 2 ] 2 (9, x, y, z), 
lK.[T 2 ]i(6, x, y, z) = 7 - 1K,[T 2 } 2 (8, x, y, z), 

4 

HC[T 2 ] 5 (6,x,y,z) = 8 - lK,[T 2 ]i(0, x, y, z), 

i=l 

where p = \J ol 2 + (3 2 , s = sin(7/2), c = cos(7/2) and 
j=(z-d 1 6)/(d 2 -d 1 ), 



I 



p(l + c) + sign( 7 ) v /p 2 (l + c) 2 - (1 + c)(2p 2 - 8s 2 ) 



|| (tii - d2,ci - c 2 ) 
x 
V 



2(1 + c) 

di - d 2 
ci - c 2 

-di ci 
ci di 



c 2 
di 



ci 
d 2 



1 — cos ( 
sin 6* 



Then, 1K[T 2 ] is a local right inverse of FK.^ 1 ' 2 ' 1 ' 2 ^ , that is, 
it satisfies jf/C (1 ' 2,1 ' 2,1) o 2X[T 2 ] = i&u : U -f C/. 



ci - c 2 6 a - b 2 
6 2 - 61 ci - c 2 

costi - cos(ti + £ 2 ) + cos(ii + t 2 + t 3 ) - cos(£)? =1 tj) 
sinti - sin(ti + t 2 ) + sin(£i + £ 2 + £3) - sin(Z) i= i *i) . 

After some rather involved computations, one can verify 
^(1,2,1,2,1) ( tl ,t 2 ,t 3 ,ti,t 5 ) = {e,x,y,z). U 

B. Three-dimensional input distribution 

Let V\, V 2 , V 3 satisfy the controllability condition in 
LemmaHV21 Consider TIC {1 - 3 ' 21) : K 4 -> SE(2) x R. 

Proposition TV.7: (Inversion for 73-systems on SE(2) x 
R). Let (V 1 ,V 2l V 3 ) G T 3 . Consider the map 
X1C[T 3 } : SE(2) xl->I 4 whose components are 

XIC[T 3 ]i(8,x,y, z) = arctan2 (a, (3) - llC[T 3 } 2 (8,x, y, z) , 

UC[T 3 ] 2 (6,x,y,z) = - — 

«3 — «1 

UC[T 3 } 3 (8,x,y,z) = p, 

HC[T 3 ]i(0, x,y,z) = 8 — arctan2 (a, (3) , 



where p = \J a 2 + (3 2 and 



a 


1 


b 2 c 2 




X 




-a bi 




1 — cos 8 




A 


~ % + c\ 


-c 2 6 2 _ 


( 


y. 




bi ci 




sin 8 


) 



Then, TK[T 3 ] is a global right inverse of _F/C (1 ' 3,2,1) , that is, 
it satisfies J r /C (1 ' 3 ' 2 ' 1) oZ/C[T 3 ] = id SE ( 2 ) X R: SE(2) xl^ 
SE(2) x R. 

Proof: The proof follows from the expression of 

^£(1,3,2,1) _ if jr^:^' 3,2 ' 1 ' (ti, t 2 , *s, **) = {0, x,y,z), then 

6» = ii + i 2 + *4 , 

z = d\t\ + d 3 t 2 + d\ti = d\6 + (d 3 - d\)t 2 , 



X 




-C\ bi 




1 — cos 8 




"b 2 


-c 2 




cos(ti - 


Ma)' 


}l_ 




bi ci_ 




sin 8 


+ 


c 2 


6 2 _ 




sin(tx - 


ht 2 )_ 



The equation in [x, y] T can be rewritten as 

*3, 








cos(ii - 


vt 2 ) 


A 




sin(ti - 


ht 2 ) 



which is solved by the selection given by (£1, £ 2 , t 3 , £4) 
T/C[T 3 ](0,x,y,2). 
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Consider the map ^/c (1 ' 2 ' li3) : R 4 -> SE(2) x M. 

Proposition IV.8: (Inversion for Xj-systems on SE(2) x 
R). Let {Vi,V 2 ,V 3 ) G Consider the map 

I/C[X 4 ] : SE(2) xl^l 4 given by 



X/C[Xi] (9, x, y,z) = ( arctan2 (a, (3) , p, 

8 — arctan2 (a, f3) 

where p = a 2 + f3 2 and 



z - d x e 






1 


62 c 2 




a; 




-Ci 61 




1 — COS 


) 


A 


" b\ + 4 


-c 2 rf 2 _ 


( 


y. 




61 Cl_ 




sin# 





Then, lfC[T 4 ] is a global right inverse of XX' 1 ' 2 ' 1 ^, that is, 
it satisfies XX (1 ' 2J ' 3) oX/C[X 4 ] = id SE(2)xR : SE(2) x R -► 
SE(2) x R. 

Proof: If X7C (1,2 ' 1,3) (ti,t 2 ,*3,*4) = (0, x, y, z), then 



= *1 + *3 , 



.7; 








1 — COS 




b 2 -c 2 




COS^i 


..'/_ 




61 Cl_ 




sin 


+ 


c 2 6 2 _ 




sinii 



2 = dx(ti + t 3 ) + d 3 t 4 . 



The equation in [x,y] T can be rewritten as [a,/3] T = 
[costi, sinii] T i 2 . As in the proof of Proposition 111.21 the 
selection t\ = arctan2 (a, (3), t 2 = p solves it. ■ 
Proposition IV.9: (Inversion for Xs-systems on SE(2) x 
R). Let (V 1 ,V2,V 3 ) G %. Define the neighborhood of the 
identity in SE(2) x R 

U = {{0,x,y) G SE(2) x R| ||( Cl - c 2 ,b x - 6 2 )|| 2 > 

max{||(x,y)|| 2 ,2(l-cos0)||(6i, Cl )|| 2 }. 

Consider the map 1K[%] : U C SE(2) xl^l 4 whose 
components are 

IK\r$]\(6, x, y, z) = arctan2 ^p, \f& — p 2 ^ + arctan2 (a, (3) . 

1K.[T 5 } 2 (6, x, y, z) = arctan2 (2 - p 2 ,p v / 4-p 2 ^) , 
XX[X 5 ] 3 (0, x, y,z) = e- 1K[%] X {9, x, y) - X/C[X 5 ] 2 (0, x, y) , 

lK[K\i(6,x,y,z) = dld , 



Let Z7Cp5](#, x, y, z) = (ti,t 2 ,t 3 ,t4,). The components of 
X/C (1 ' 2 ' 1 ^(ti,t2^3,i 4 )are 



i^ci mi\,^5^u, j,, y, — yvi 

X/C (1 ' 2 ' 1 ^(ti,t2^3,i 4 )are 

^^-(1,2,1,3) (t ll t 2 ,t 3 ,t i ) = 
1 1 1 1 i\ 







Ci 61 




1 — cos 






6 


1 Ci 




sin# 




-6a 




costi 




cos(ti + t 


2) 


- c 2 




sini! 




sin(<! + 1 


2) 



a - c 2 bi 

b 2 - bi 

^ 1,2,1 * 3) (t lj t 2 , t 3 , t 4 ) = d^h + t 2 + 1 3 ) + d 3 t 4 



J-l\, 4 L 2 ,L 3 ,Lij — "1^1 T 2 -T U 3 ) -T U 3 U4 . 

One can verify that X"/C (1 ' 2 ' 1 ' 3) (ti,i 2 ,i 3 ,i 4 ) = (0,x,y,z). ■ 
We illustrate the performance of the algorithms in Fig.[3] 




and p = \J a 2 + (3 2 and 
1 

6 



(c 1 -c 2 ,6 1 -6 2 )|| 2 



ci — c 2 b 2 — bi 
bi -6 2 Ci - c 2 





X 




-Ci 61 




1 — cos 9 




( 


_.'/_ 




h ci_ 




sin 6* 


) 



Then, X/C[X 5 ] is a local right inverse of XX (1,2 ' 1,3) , that is, 
it satisfies X7C (1 ' 2,1 ' 3) o XX[Xg] = idry : C/ -> 17. 

Proo/v If (0, x, y, z) G £/, then one can see that p = 
|| (a, /3) || < 2, and therefore XX [T 5 ] is well-defined on J7. 



Fig. 3 

We illustrate the inverse-kinematics planner FOR A Ti -system 
on SE(2) x E. The system parameters are 61 = 1, ci = 0, di = .5, 
b 2 = -2, and c 2 = 0. The target location is (71-/6, 10, 0, 1). 



V. Conclusions 

We have presented a catalog of feasible motion plan- 
ning algorithms for underactuated controllable systems 
on SE(2), SO(3) and SE(2) x R. Future directions of re- 
search include (i) considering other relevant classes of un- 
deractuated systems on SE(3), (ii) computing catalogs of 
optimal sequences of motion primitives, and (iii) devel- 
oping hybrid feedback schemes that rely on the proposed 
open-loop planners to achieve point stabilization and tra- 
jectory tracking. 
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